#ifndef RVIZ_PLANNER__PLANNER_HPP_
#define RVIZ_PLANNER__PLANNER_HPP_

#include <fstream>
#include <vector>
#include <queue>
#include <unordered_map>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
// 自定义
#include "nav_msg_pkg/msg/waypoint.hpp"
#include "rviz_util_pkg/rviz_marker.hpp"
#include "agv_util/point_util.hpp"
#include "rviz_planner/fixed.hpp"
#include "rviz_planner/costmap.hpp"

namespace rviz_planner
{
    class Planner : public nav2_util::LifecycleNode
    {
    public:
        Planner(const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
        ~Planner() override;

    protected:
        nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override;

        nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override;

        nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override;

        nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override;

        nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override;

        void initial_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
        void goal_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);

        void publish_marker();

        void publish_path();
        std::vector<nav_msg_pkg::msg::Waypoint> get_neighbor_nodes_by_name(std::string name);

    protected:
        bool updateWaypoints();

        //计算从起始点到目标点的弧度差（起始点有方向，目标点无方向）
        void calculate_radian();

    private:
        rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_; // 发布路径

        std::unique_ptr<rviz_util_pkg::RvizMarker> rviz_marker_ptr_;                                      // 发布rviz的marker
        rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_; // 初始位置订阅
        rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_sub_;                  // 目标位置订阅

        std::vector<nav_msg_pkg::msg::Waypoint> points_;
        std::vector<agv_util::WaypointNeighbors> waypoint_neighbors_; // 节点邻接表

        geometry_msgs::msg::Pose start_pose_;
        geometry_msgs::msg::Pose goal_pose_;

        std::unique_ptr<rviz_planner::Fixed> planner_;
        std::unique_ptr<Costmap> costmap_;
    };
} // namespace rviz_planner
#endif // RVIZ_PLANNER__PLANNER_HPP_